#include "ros/ros.h"
#include <tf/transform_listener.h>
#include <tf/tf.h>
#include <cmath>
#include <iostream>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include "sensor_msgs/JointState.h"

using namespace std;



int main (int argc, char** argv)
{
    ros::init(argc, argv, "tf_dudu");
    ros::NodeHandle n;          
    
    tf::TransformListener listenser;
    tf::StampedTransform transform;

    cout<<"lintenser is here "<<endl;
    

    ros::Rate loop_rate(10);
    

    while(ros::ok())
    {
        listenser.waitForTransform("/map", "odom", ros::Time(0), ros::Duration(4.0));
        try
        {
            cout<<"listen is run "<<endl;
            listenser.lookupTransform("/map", "odom", ros::Time(0), transform);

            // X Y 数据截取 
            double x = transform.getOrigin().getX();
            double y = transform.getOrigin().getY();

            // 需要对 X Y 数据操作 就放这里

                // 发布 位置

                // 其他操作


            // 自定义X Y 操作结束 

        }
        catch(const tf::TransformException &e)
        {
            std::cerr << e.what() << '\n';
            ros::Duration(1.0).sleep();
            continue;
        }
        


        std::cout<<"输出的位置坐标:x="<<transform.getOrigin().x()<<",y="<<transform.getOrigin().y()<<std::endl;

        std::cout<<" TF数据截取完毕咯! "<<std::endl;

        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}
